#include "plugins/action/find_toe_off.hpp"

BT::NodeStatus FindToeOff::onStart()
{
    std::cout << "FindToeOff_onStart(): " << this->name() << std::endl;
    cnt = 0;
    /*从端口获取数据*/
    if (!getInput<uint64_t>("T_hs", T_hs_)) {
        throw BT::RuntimeError("Missing required input 'FindToeOff.T_hs'");
    }
    if (!getInput<std::shared_ptr<std::vector<uint64_t>>>("mean_stance_duration", mean_stance_duration_ptr)) {
        throw BT::RuntimeError("Missing required input 'FindToeOff.mean_stance_duration_'");
    }
    //从端口的指针获取mean_stance_duration_数组的数据
    if (mean_stance_duration_ptr) {
        mean_stance_duration_ = *mean_stance_duration_ptr;
    } else {
        std::cout<<COLOR_RED << "mean_stance_duration_ptr is null!" << COLOR_RESET<< std::endl;
    }
    // 获取数组mean_stance_duration_最后一个数值
    try {
        // 可能抛出异常的代码（如访问 vector 的 back() 前检查 empty()）
        if (mean_stance_duration_.empty()) {
            throw std::runtime_error("Vector is empty!");
        }
    last_stance_duration_ = mean_stance_duration_.back();
    } catch (const std::exception& e) {  // 正确：catch 紧跟 try
        std::cerr << "Error: " << e.what() << std::endl;
    }
    // std::cout << "Last last_stance_duration_ value: " << last_stance_duration_ << std::endl;

    print_all_inports(true);
    this->onRunning();
    return BT::NodeStatus::RUNNING;
}



BT::NodeStatus FindToeOff::onRunning(){
    /*从端口获取数据*/
    if (!getInput<sensorData>("sensor_input", tmp_sensor_data_filtered_)) {
        throw BT::RuntimeError("Missing required input 'FindToeOff.sensor_input'");
    }


    /*获取当前时间ms*/
    auto now = std::chrono::steady_clock::now();
    auto duration = now.time_since_epoch();
    // current_millis_ = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
    // current_millis_ = TimeUtils::TimestampManager::record("find_toe_off.current_millis");
    // current_millis_ = std::chrono::duration_cast<std::chrono::milliseconds>(
    // std::chrono::steady_clock::now().time_since_epoch()).count();
    current_millis_ = config().blackboard->get<uint64_t>("global_counter");
    current_millis_ *= 2;
    tof_distance_ = tmp_sensor_data_filtered_.tof_distance;
    ////调试逻辑用
    // cnt++;
    // if (cnt > 3)
    // {
    //     return BT::NodeStatus::SUCCESS;
    // }
    // std::cout<<"FindToeOff_onRunning"<<std::endl;
    // return BT::NodeStatus::RUNNING;


    // todo 考虑处理一些T_hs_的数据异常
    if (current_millis_-T_hs_ > 0.4*last_stance_duration_){
        if (tof_distance_last_ < 0 && tof_distance_ >= 0)
        {   
            // std::cout<<"FindToeOff_tof_distance_last_ < 0 && tof_distance_ >= 0"<< "current_millis_"<< current_millis_ <<std::endl;
            // return BT::NodeStatus::FAILURE;
            //当前时刻点作为T_to: toe off
            T_to_ = current_millis_;
            std::cout << COLOR_YELLOW << "FindToeOff_T_to_: " << T_to_ << COLOR_RESET << std::endl;
            setOutput("T_to", T_to_);// 开始脚尖抬起,todo 给出toeoff的标志位
            config().blackboard->set("FindToeOff_T_to", current_millis_);
            // std::cout<<"FindToeOff: setOutput(T_to, current_millis_) current_millis_:"<< current_millis_ <<std::endl;

            //参照原文 计算stance_duration_
            stance_duration_ = current_millis_ - T_hs_; //下一步更新到均值的数组中
            //todo: 更新mean_stance_duration_数组
            mean_stance_duration_.push_back(stance_duration_); //将新值追加到mean_stance_duration_数组
            setOutput("mean_stance_duration", std::make_shared<std::vector<uint64_t>>(mean_stance_duration_)); //将数组通过端口发布出去
            // TimeUtils::TimestampManager::print_all_timestamps();
            this->print_all_outports(true);
            return BT::NodeStatus::SUCCESS;
        }

        tof_distance_last_ = tof_distance_;
        // std::cout<<"FindToeOff: current_millis_-T_hs_ > 0.4*last_stance_duration_"<<std::endl;
        return BT::NodeStatus::RUNNING;

    }
    else{
        // std::cout<<"FindToeOff: else"<<std::endl;
        tof_distance_last_ = tof_distance_;
        return BT::NodeStatus::RUNNING;
    }

    
}

void FindToeOff::print_vector(const std::vector<uint64_t>& vec, const std::string& name) {
    if (!vec.empty()) {
        std::cout << name << ": [";
        for (size_t i = 0; i < vec.size(); ++i) {
            if (i > 0) std::cout << ", ";
            std::cout << vec[i];
        }
        std::cout << "]" << std::endl;
    } else {
        std::cout << name << ": 向量为空" << std::endl;
    }
}
void FindToeOff::print_var(const uint64_t& var, const std::string& name) {
    std::cout << name << ": " << var << std::endl;
}


void FindToeOff::print_all_outports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindToeOff节点端口输出值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_to_, "T_to_");
    // print_vector(mean_T_hs_potential_region_, "mean_T_hs_potential_region_");
    print_vector(mean_stance_duration_, "mean_stance_duration_");
    std::cout << "======================" << std::endl;
}
void FindToeOff::print_all_inports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindToeOff节点端口输入值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_hs_, "T_hs_");
    // 打印所有向量
    print_vector(mean_stance_duration_, "mean_stance_duration_");
    std::cout << "======================" << std::endl;
}


















#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  std::cout << "Registering nodes...: FindToeOff" << std::endl;//在宏内添加调试信息或条件判断
  factory.registerNodeType<FindToeOff>("FindToeOff");
}